import numpy as np
from math import atan2, pi, exp
from algo import rad_limit, vec_cross, vector_dot_angle, norm


# ----------------------------------- PATH TRACKING METHOD START -----------------------------------
def line_tracking(control, xy):
	'''

	:param control: ControlParams类
	:param set_u:
	:param xy:
	:return:
	'''
	# print('xy', xy)
	edx = np.zeros(3, dtype=float)
	vec = [xy[control.flag][0] - control.x[0], xy[control.flag][1] - control.x[1], xy[control.flag][2] - control.x[2]]
	# print('xy current point', [xy[flag][0], xy[flag][1], xy[flag][2]])
	# print('vec', vec)
	# print('flag:', flag)
	if norm(vec) < 6:
		control.flag += 1

	# tracking
	# guider
	if norm(vec) < 10:
		#  减速 看需要使用
		b_u = control.target_u
	else:
		b_u = control.low_u
	control = uuv_s(vec, control)
	return control


# ----------------------------------- PATH TRACKING METHOD END -----------------------------------

# ----------------------------------- COLLISION AVOIDANCE METHOD START -----------------------------------

def main_cv(control, objs, coe):
	'''
	避障入口程序 用于计算位置误差
	:param control: 结构体
	:param objs: {N}位置
	:param coe: APFCoe
	:return:
	'''
	dis = norm(coe.goal - control.x[0:3])
	# print('dis is :', dis)
	if dis < coe.end_dis:
		control.flag = 1
		return control
	else:
		control.flag = 0
	next_vec = potential_field(control, objs, coe)
	# print('dis_all_obj: ', dis_all_obj)
	control = uuv_s(next_vec, control)
	return control


def potential_field(control, objs, coe):
	'''

	:param control: 控制参数
	:param objs: 障碍物
	:param coe: 避障参数
	:param dis_all_obj: 记录到所有障碍物的距离
	:return:
	'''
	obj_vec = np.zeros(shape=[len(objs), 3])
	goal_l = np.array([coe.goal[0] - control.x[0], coe.goal[1] - control.x[1], coe.goal[2] - control.x[2]])
	dis = np.zeros(len(objs))
	i = 0
	for obj in objs:
		obj_l = np.array([control.x[0] - obj.x[0], control.x[1] - obj.x[1], control.x[2] - obj.x[2]])
		dis[i] = norm(obj_l)
		# 记录到第i个障碍的距离
		# 指定规则避障
		obj_cross = ca_rules(control.nu[0:3], obj.nu[0:3])
		obj_cross = vec_cross(obj_l, obj_cross)
		obj_cross = obj_cross + coe.c_obj * obj_l
		if norm(obj_l) < obj.danger_r:
			# print('obj_l', obj_l)
			if norm(goal_l) < obj.danger_r:
				coe_goal_dis = norm(goal_l) / (obj.danger_r ** 3)
			else:
				coe_goal_dis = 1
			obj_f = coe_goal_dis * 0.5 * coe.obj_eta * \
			        (1 / norm(obj_l) - 1 / obj.danger_r) * \
			        ((1 / norm(obj_l)) ** 2)
			obj_vec[i, :] = obj_f * obj_cross / norm(obj_cross)
		else:
			obj_vec[i, :] = [0, 0, 0]
		i += 1

	obj_f_sum = sum(obj_vec)
	# 斥力矢量叠加
	goal_f = np.zeros(3)
	if norm(goal_l) < coe.goal_r:
		goal_f = 10 * coe.goal_k / norm(goal_l)
	else:
		goal_f = coe.goal_k / norm(goal_l)
	goal_vec = (goal_f * goal_l / norm(goal_l)) + obj_f_sum
	# 选择性强化目标点附近的引力场
	return goal_vec


def ca_rules(boat_v, obj_v):
	'''
	避障规则
	:param boat_v: 船的速度矢量 ndarry
	:param obj_v: 障碍的速度矢量 ndarry
	:return: 返回规避运动方向
	'''
	# 计算相对速度
	obj_rela = obj_v - boat_v

	vec_angle = vector_dot_angle(obj_rela, np.array([1, 0, 0])) - vector_dot_angle(obj_rela, np.array([1, 0, 0]))
	cv_angle = vec_cross(boat_v, obj_v)
	# 船速度到障碍物速度的夹角 后面再补充

	obj_cross = [0, 0, 1]

	return obj_cross


# ----------------------------------- COLLISION AVOIDANCE METHOD END -----------------------------------

# ----------------------------------- PID CONTROLLER START -----------------------------------


def uuv_s(next_vec, control):
	'''
	控制方法入口
	:param next_vec: 期望矢量方向
	:param control: 控制参数
	:param b_u: 期望速度
	:return:
	'''
	# z = np.zeros(3,dtype=float)
	# delta = np.zeros(3, dtype=float)
	# f = np.zeros(6, dtype=float)
	edx = np.zeros(3)
	b_angle_z = atan2(next_vec[1], next_vec[0])
	b_angle_y = -atan2(next_vec[2], norm(next_vec))
	alpha = -atan2(control.u[2], control.u[0])
	beta = atan2(control.u[1], control.u[0])

	edx[0] = control.target_u - norm(control.u[0:3])
	edx[1] = rad_limit(b_angle_y - (control.x[4] + alpha))
	edx[2] = rad_limit(b_angle_z - (control.x[5] + beta))

	z, control = uuv_s2(edx, control)
	# 桨舵 35deg
	delta_max = 25 * pi / 180
	F_max = 500
	M_max = 500
	if control.f_mode == 1:
		# 桨舵
		con_vars = [F_max * z[0], delta_max * z[2], delta_max * z[1]]
		# 做成返回值？
		control.f[0] = con_vars[0]
		# delta_r - -psi，航向，垂直舵
		control.delta[0] = con_vars[1]
		# delta_s - -theta，纵倾，水平舵
		control.delta[2] = con_vars[2]
	# print('delta:', delta)
	elif control.f_mode == 2:
		# 双推进器
		con_vars_t = [F_max * z[0], M_max * z[2], 0]
		control.f[0] = con_vars_t[0]
		control.f[5] = con_vars_t[1]
	control.old2edx = control.oldedx
	control.oldedx = edx
	return control


def uuv_s2(edx, control):
	'''
	控制器 误差输入 驱动量输出
	:param edx:
	:param control:
	:return:
	'''
	z = [0, 0, 0]
	if control.mode == 1:
		# 桨舵
		kp1 = 1
		ki1 = 0.1
		kd1 = 0.01
		kp2 = 1
		ki2 = 0.005
		kd2 = 0.1
		kp3 = 0.5
		ki3 = 0
		kd3 = 0.1
	elif control.mode == 2:
		# 双尾推
		kp1 = 0.1
		ki1 = 0.1
		kd1 = 0.5
		kp2 = 1
		ki2 = 0.005
		kd2 = 0.1
		kp3 = 0.1
		ki3 = 0
		kd3 = 0.2

	pid_u1 = kp1 * (edx[0] - control.oldedx[0]) + ki1 * edx[0] + kd1 * (
			edx[0] - 2 * control.oldedx[0] + control.old2edx[0])
	control.pidu[0] = control.pidu[0] + pid_u1
	pid_u2 = kp2 * (edx[1] - control.oldedx[1]) + ki2 * edx[1] + kd2 * (
			edx[1] - 2 * control.oldedx[1] + control.old2edx[1])
	control.pidu[1] = control.pidu[1] + pid_u2
	pid_u3 = kp3 * (edx[2] - control.oldedx[2]) + ki3 * edx[2] + kd3 * (
			edx[2] - 2 * control.oldedx[2] + control.old2edx[2])
	control.pidu[2] = control.pidu[2] + pid_u3

	z[0] = 2 / (1 + exp(- control.pidu[0])) - 1
	# theta, 纵倾, 使用S面的效果更好，明显不抖动了，超调也变小
	z[1] = 2 / (1 + exp(- control.pidu[1])) - 1
	# psi, 航向
	# print('edx', edx)
	z[2] = 2 / (1 + exp(- control.pidu[2])) - 1
	# print('z', z)
	return z, control
# ----------------------------------- PID CONTROLLER END -----------------------------------


# ----------------------------------- TEST START-----------------------------------
